/*
 * Includes
 */ 
#include <x32.h>
#include <ucos.h>


/*
 * Global Definitions
 */ 
#define PWM_PERIOD 				peripherals[PERIPHERAL_DPC1_PERIOD]
#define PWM_WIDTH 				peripherals[PERIPHERAL_DPC1_WIDTH]
#define SSD 					peripherals[PERIPHERAL_DISPLAY]
#define ENGINE_A 				peripherals[PERIPHERAL_ENGINE_A]
#define ENGINE_B  				peripherals[PERIPHERAL_ENGINE_B]
#define TIME_SLICE 				1
#define ENGINE_DECODED			peripherals[PERIPHERAL_ENGINE_DECODED]


// uC/OS PRIORITIES
// higher number is higher priority
#define PERIODIC_TASK_PRIORITY	10
#define ERROR_TASK_PRIORITY 	9


//x32 ISR PRIORITIES
// lower number is higher priority
#define PRIORITY_ENGINE_ERROR 	2
#define PRIORITY_BUTTONS 		5

// LEDs Definitions
#define LED7 0x80
#define LED6 0x40
#define LED5 0x20
#define LED4 0x10
#define LED3 0x08
#define LED2 0x04
#define LED1 0x02
#define LED0 0x01

// //BUTTONS Definitions
#define BTN3 0x08
#define BTN2 0x04
#define BTN1 0x02
#define BTN0 0x01

/*
 Global Variables
 */
int throttle = 0, old_count=0, speed=0;
int stack1[1024], stack2[256];
OS_EVENT *sem_error;
UBYTE err;

void isr_buttons() { 										// Button interupt service
	OSIntEnter();											// OSIntEnter stops OS temporarily to stop context switching	
	if (peripherals[PERIPHERAL_BUTTONS] & BTN3)	{			// Reset button, left most on board.
		SSD =0x00;											// Reset screen to zeros
		PWM_WIDTH=0x00;										// Reset PWM to zeros
		exit();												// Exit OS to bootloader
	}  
	else if (peripherals[PERIPHERAL_BUTTONS] & BTN0) {		// Increase button, right most on board.
		if (throttle <= 0xFFFB) throttle += 4;
	}
	else if (peripherals[PERIPHERAL_BUTTONS] & BTN1) { 		// Deacrease button, right middle button on board.
		if (throttle >= 0x0004) throttle -= 4;
	}
	OSIntExit(); 											// Restore OS context switching
}

void isr_error() {
	OSIntEnter();									// OSIntEnter stops OS temporarily to stop context switching				
	OSSemPost(sem_error);							// Post sem_error, any task waiting will be resumed
	OSIntExit();									// Restore OS context switching
}



void per_task(void *data) {
	while(TRUE) {
		old_count = ENGINE_DECODED; 				// Get the current count for measuring the change of count
		PWM_WIDTH = throttle;						// Assign the new throttle
		OSTimeDly(TIME_SLICE); 						// 20ms; Delay the system for one timeslice
		speed 	= ENGINE_DECODED - old_count; 		// Speed is the rate of change of count over an interval TIME_SLICE
		SSD 	= (speed << 8 ) + (throttle >> 2);	// Speed displayed on the left and throttle displayed on the right of the SSD
	}												// SSD can hold at most 16bit value.
}

void err_task(void *data) {
	while(TRUE) {
		OSSemPend(sem_error, WAIT_FOREVER, &err); 	// Pend on sem_error semaphore (Wait until posted)
		peripherals[PERIPHERAL_LEDS] |= LED7; 		// Turn on LED7
		OSTimeDly(5); 								// wait 100ms
		peripherals[PERIPHERAL_LEDS] &= ~LED7; 		// Turn off LED7
		while (OSSemAccept(sem_error) > 0);			// Restore sem_error to 0
	}
}

void main() {
	// Create Semaphores
	sem_error = OSSemCreate(0);

	// Configure the ENGINE_ERROR ISR 
	SET_INTERRUPT_VECTOR	(INTERRUPT_ENGINE_ERROR, isr_error);
	SET_INTERRUPT_PRIORITY	(INTERRUPT_ENGINE_ERROR, PRIORITY_ENGINE_ERROR);
	ENABLE_INTERRUPT		(INTERRUPT_ENGINE_ERROR);

	// Configure the BUTTONS ISR
	SET_INTERRUPT_VECTOR	(INTERRUPT_BUTTONS, isr_buttons);
	SET_INTERRUPT_PRIORITY	(INTERRUPT_BUTTONS, PRIORITY_BUTTONS);
	ENABLE_INTERRUPT		(INTERRUPT_BUTTONS);

	PWM_PERIOD = 1024;

	OSInit();

	// Lower number -> Higher priority
	OSTaskCreate(per_task, (void *)0, (void *)stack1, PERIODIC_TASK_PRIORITY);	// Priority=10
	OSTaskCreate(err_task, (void *)0, (void *)stack2, ERROR_TASK_PRIORITY);		// Priority=9

	OSStart(); //also enables the global interrupts
}



